using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using UnityEngine;


namespace PathFinding
{
    /// <summary>
    /// 地图离散化的网格，但是要注意，我们以PFPoint也就是顶点作为路径的节点，而不是格子。
    /// </summary>
    public class PFGrid : MonoBehaviour
    {
        /*                XSize
         * -----------------------------------
         * |                                        |
         * |                                        |
         * |                                        |  YSize
         * |                                        |
         * |                                        |
         * -----------------------------------
         */

        public int XSize = 200;
        public int YSize = 100;
        public int CellCount;

        public Vector2 CellSize = new Vector2Int(1, 1);

        public bool UseFourNeighbor = true;

        [Header("Test")]
        public Vector2Int StartPos = Vector2Int.zero;
        public Vector2Int GoalPos = Vector2Int.zero;
        public Transform StartPoint;
        public Transform GoalPoint;

        public enum PathFInderType
        {
            AStar,
            JPS,
            JPS_Plus,
        }
        public PathFInderType PathFinderType = PathFInderType.AStar;
        public HashSet<PFObstacle> pFObstacles = new HashSet<PFObstacle>();

        public bool SelfUpdate = true;
        public bool UpdatePathFinderMap = true;

        [Header("Debug Draw")]
        public Color StartColor = Color.green;
        public Color GoalColor = Color.yellow;

        public Color PathColor = Color.green;
        public Color PathSmoothedColor = Color.green;
        public Color OpenedCellColor = Color.magenta;
        public Color OtherClosedCellColor = Color.gray;

        public bool DrawOpenedCell = true;
        public bool DrawOtherClosedCell = true;

        public Color GridColor = Color.cyan;
        public Color ObstacleColor = Color.red;
        public bool DrawGrid = true;


        public PFPoint[,] pFPoints { get; private set; }

        IPathFInder pathFinder;
        Vector3[] pathToDraw;
        Vector3[] pathToDrawSmoothed;



        public PFPoint this[Vector2Int pos]
        {
            get
            {
                return pFPoints[pos.x, pos.y];
            }
        }

        public PFPoint this[int x, int y]
        {
            get
            {
                return pFPoints[x, y];
            }
        }

        public bool HasPointAt(in Vector2Int pos)
        {
            if (pos.x >= 0 && pos.x < XSize && pos.y >= 0 && pos.y < YSize)
                return true;
            return false;
        }

        public bool HasPointAt(int x, int y)
        {
            if (x >= 0 && x < XSize && y >= 0 && y < YSize)
                return true;
            return false;
        }

        public bool AddObstacle(PFObstacle pFObstacle)
        {
            if (pFObstacles.Add(pFObstacle))
            {
                UpdateObstacleCaptured();
                return true;
            }

            return false;
        }

        public bool RemoveObstacle(PFObstacle pFObstacle)
        {
            if (pFObstacles.Remove(pFObstacle))
            {
                UpdateObstacleCaptured();
                return true;
            }

            return false;
        }

        private void UpdateObstacleCaptured()
        {
            foreach (var ob in pFObstacles)
            {
                ob.UpdateGridPoint();
            }
        }

        // Start is called before the first frame update
        void Awake()
        {
            if (XSize < 3 || YSize < 3)
                throw new Exception("Too small Grid");

            CellCount = XSize * YSize;

            pFPoints = new PFPoint[XSize, YSize];

            var rand = new System.Random();

            for (int x = 0; x < XSize; ++x)
            {
                for (int y = 0; y < YSize; ++y)
                {
                    pFPoints[x, y] = new PFPoint()
                    {
                        Pos = new Vector2Int(x, y),
                        Neighbors = UseFourNeighbor ? GetNeighbor4(new Vector2Int(x, y)) : GetNeighbor(new Vector2Int(x, y)),
                        Roughness = rand.Next(0, 7)
                    };
                }
            }

            if (pFObstacles != null && pFObstacles.Any())
            {
                UpdateObstacleCaptured();
            }

            switch (PathFinderType)
            {
                case PathFInderType.AStar: pathFinder = new AStarPathFinder(this); break;
                case PathFInderType.JPS: pathFinder = new JPSPathFinder(this); break;
                case PathFInderType.JPS_Plus: pathFinder = new JPSPathFinderCached(this); break;

            }

            pathToDraw = null;

            GenMesh();
        }

        Vector2Int[] GetNeighbor(Vector2Int pos)
        {
            if (pos.x >= XSize || pos.x < 0 || pos.y >= YSize || pos.y < 0)
                throw new Exception("pos " + pos + " not in grid map");

            List<Vector2Int> points = new List<Vector2Int>(3);

            if (pos.x == 0)
            {
                // R
                points.Add(new Vector2Int(pos.x + 1, pos.y));

                if (pos.y == 0)
                {
                    // B
                    points.Add(new Vector2Int(pos.x, pos.y + 1));

                    // BR
                    points.Add(new Vector2Int(pos.x + 1, pos.y + 1));

                }
                else if (pos.y == YSize - 1)
                {
                    // T
                    points.Add(new Vector2Int(pos.x, pos.y - 1));

                    // TR
                    points.Add(new Vector2Int(pos.x + 1, pos.y - 1));
                }
                else
                {
                    // B
                    points.Add(new Vector2Int(pos.x, pos.y + 1));
                    // T
                    points.Add(new Vector2Int(pos.x, pos.y - 1));

                    // BR
                    points.Add(new Vector2Int(pos.x + 1, pos.y + 1));
                    // TR
                    points.Add(new Vector2Int(pos.x + 1, pos.y - 1));
                }
            }
            else if (pos.x == XSize - 1)
            {
                // L
                points.Add(new Vector2Int(pos.x - 1, pos.y));

                if (pos.y == 0)
                {
                    // B
                    points.Add(new Vector2Int(pos.x, pos.y + 1));

                    // BL
                    points.Add(new Vector2Int(pos.x - 1, pos.y + 1));

                }
                else if (pos.y == YSize - 1)
                {
                    // T
                    points.Add(new Vector2Int(pos.x, pos.y - 1));

                    // TL
                    points.Add(new Vector2Int(pos.x - 1, pos.y - 1));
                }
                else
                {
                    // B
                    points.Add(new Vector2Int(pos.x, pos.y + 1));
                    // T
                    points.Add(new Vector2Int(pos.x, pos.y - 1));

                    // BL
                    points.Add(new Vector2Int(pos.x - 1, pos.y + 1));
                    // TL
                    points.Add(new Vector2Int(pos.x - 1, pos.y - 1));
                }
            }
            else
            {
                // R
                points.Add(new Vector2Int(pos.x + 1, pos.y));
                // L
                points.Add(new Vector2Int(pos.x - 1, pos.y));

                if (pos.y == 0)
                {
                    // B
                    points.Add(new Vector2Int(pos.x, pos.y + 1));

                    // BR
                    points.Add(new Vector2Int(pos.x + 1, pos.y + 1));
                    // BL
                    points.Add(new Vector2Int(pos.x - 1, pos.y + 1));

                }
                else if (pos.y == YSize - 1)
                {
                    // T
                    points.Add(new Vector2Int(pos.x, pos.y - 1));

                    // TR
                    points.Add(new Vector2Int(pos.x + 1, pos.y - 1));
                    // TL
                    points.Add(new Vector2Int(pos.x - 1, pos.y - 1));
                }
                else
                {
                    // B
                    points.Add(new Vector2Int(pos.x, pos.y + 1));
                    // T
                    points.Add(new Vector2Int(pos.x, pos.y - 1));

                    // BR
                    points.Add(new Vector2Int(pos.x + 1, pos.y + 1));
                    // TR
                    points.Add(new Vector2Int(pos.x + 1, pos.y - 1));
                    // BL
                    points.Add(new Vector2Int(pos.x - 1, pos.y + 1));
                    // TL
                    points.Add(new Vector2Int(pos.x - 1, pos.y - 1));
                }
            }

            return points.ToArray();
        }

        Vector2Int[] GetNeighbor4(Vector2Int pos)
        {
            if (pos.x >= XSize || pos.x < 0 || pos.y >= YSize || pos.y < 0)
                throw new Exception("pos " + pos + " not in grid map");

            List<Vector2Int> points = new List<Vector2Int>();

            if (pos.x == 0)
            {
                // R
                points.Add(new Vector2Int(pos.x + 1, pos.y));
            }
            else if (pos.x == XSize - 1)
            {
                // L
                points.Add(new Vector2Int(pos.x - 1, pos.y));
            }
            else
            {
                // R
                points.Add(new Vector2Int(pos.x + 1, pos.y));
                // L
                points.Add(new Vector2Int(pos.x - 1, pos.y));
            }

            if (pos.y == 0)
            {
                // B
                points.Add(new Vector2Int(pos.x, pos.y + 1));
            }
            else if (pos.y == YSize - 1)
            {
                // T
                points.Add(new Vector2Int(pos.x, pos.y - 1));
            }
            else
            {
                // B
                points.Add(new Vector2Int(pos.x, pos.y + 1));
                // T
                points.Add(new Vector2Int(pos.x, pos.y - 1));
            }

            return points.ToArray();
        }

        Vector2Int lastStart = Vector2Int.zero;
        Vector2Int lastGoal = Vector2Int.zero;

        // Update is called once per frame
        void Update()
        {
            if (!SelfUpdate)
                return;

            if (StartPoint != null)
            {
                StartPos = new Vector2Int((int)(StartPoint.position.x / CellSize.x), (int)(StartPoint.position.z / CellSize.y));
            }
            if (GoalPoint != null)
            {
                GoalPos = new Vector2Int((int)(GoalPoint.position.x / CellSize.x), (int)(GoalPoint.position.z / CellSize.y));
            }

            GoalPos.x = Math.Clamp(GoalPos.x, 0, (int)XSize - 1);
            GoalPos.y = Math.Clamp(GoalPos.y, 0, (int)YSize - 1);
            StartPos.x = Math.Clamp(StartPos.x, 0, (int)XSize - 1);
            StartPos.y = Math.Clamp(StartPos.y, 0, (int)YSize - 1);
            if (StartPos == GoalPos)
                return;

            if (lastGoal != GoalPos || lastStart != StartPos)
            {
                if (UpdatePathFinderMap)
                    pathFinder.UpdateMap();


                if (pathFinder.FindPath(StartPos, GoalPos))
                {
                    pathToDraw = new Vector3[pathFinder.CurrentPath.Count];
                    for (int i = 0; i < pathToDraw.Length; ++i)
                    {
                        pathToDraw[i] = new Vector3(pathFinder.CurrentPath[i].x * CellSize.x, 0, pathFinder.CurrentPath[i].y * CellSize.y);
                    }
                    pathToDrawSmoothed = new Vector3[pathFinder.SmoothedPath.Count];
                    for (int i = 0; i < pathToDrawSmoothed.Length; ++i)
                    {
                        pathToDrawSmoothed[i] = new Vector3(pathFinder.SmoothedPath[i].x * CellSize.x, 0, pathFinder.SmoothedPath[i].y * CellSize.y);
                    }
                }
                else
                {
                    pathToDraw = null;
                    pathToDrawSmoothed = null;
                    Debug.Log("Can't Find A Path to Goal");
                }

                lastGoal = GoalPos;
                lastStart = StartPos;
            }


        }

        public bool CallAPath(Vector3 start, Vector3 goal, ref Vector3[] path)
        {
            var startPos = new Vector2Int((int)(start.x / CellSize.x), (int)(start.z / CellSize.y));
            var goalPos = new Vector2Int((int)(goal.x / CellSize.x), (int)(goal.z / CellSize.y));
            goalPos.x = Math.Clamp(goalPos.x, 0, (int)XSize - 1);
            goalPos.y = Math.Clamp(goalPos.y, 0, (int)YSize - 1);
            startPos.x = Math.Clamp(startPos.x, 0, (int)XSize - 1);
            startPos.y = Math.Clamp(startPos.y, 0, (int)YSize - 1);

            pathFinder.UpdateMap();


            if (pathFinder.FindPath(startPos, goalPos))
            {
                path = new Vector3[pathFinder.CurrentPath.Count];
                path[0] = start;
                Debug.Log("Path:" + pathFinder.CurrentPath.Count);

                for (int i = 1; i < path.Length - 1; ++i)
                {
                    path[i] = new Vector3(pathFinder.CurrentPath[i].x * CellSize.x, 0, pathFinder.CurrentPath[i].y * CellSize.y);
                }
                path[path.Length - 1] = goal;

                pathToDraw = new Vector3[pathFinder.CurrentPath.Count];
                for (int i = 0; i < pathToDraw.Length; ++i)
                {
                    pathToDraw[i] = new Vector3(pathFinder.CurrentPath[i].x * CellSize.x, 0, pathFinder.CurrentPath[i].y * CellSize.y);
                }

                return true;
            }
            else
            {
                Debug.Log("Can't Find A Path to Goal " + startPos  + " " + goalPos);
                path = null;
                return false;
            }
        }


        private void OnDrawGizmos()
        {
            var tl = Vector3.zero;
            var color = GridColor;

            // Draw Grid
            if (DrawGrid)
            {
                for (int x = 0; x < XSize; ++x)
                {
                    Debug.DrawLine(tl + new Vector3(x * CellSize.x, 0, 0), tl + new Vector3(x * CellSize.x, 0, YSize * CellSize.y - CellSize.y), color);
                }
                for (int y = 0; y < YSize; ++y)
                {
                    Debug.DrawLine(tl + new Vector3(0, 0, y * CellSize.y), tl + new Vector3(XSize * CellSize.x - CellSize.x, 0, y * CellSize.y), color);
                }
            }

            /// Draw Path
            if (pathFinder != null && pathToDraw != null)
            {

                if (pathFinder.OpenedPoints != null && DrawOpenedCell)
                {
                    //Debug.Log(pathFinder.OpenedPoints.Count);
                    Gizmos.color = OpenedCellColor;
                    foreach (var opened in pathFinder.OpenedPoints)
                    {
                        Gizmos.DrawCube(new Vector3(opened.x * CellSize.x, 0, opened.y * CellSize.y), new Vector3(CellSize.x, 0.2f, CellSize.y));
                    }

                }
                if (pathFinder.OtherClosedPoints != null && DrawOtherClosedCell)
                {
                    //Debug.Log(pathFinder.OtherClosedPoints.Count);

                    Gizmos.color = OtherClosedCellColor;
                    foreach (var closed in pathFinder.OtherClosedPoints)
                    {
                        Gizmos.DrawCube(new Vector3(closed.x * CellSize.x, 0, closed.y * CellSize.y), new Vector3(CellSize.x, 0.1f, CellSize.y));
                    }
                }


                Vector3 lastPos = pathToDraw[0];
                //Debug.DrawLine(pathToDraw[0], pathToDraw[0] + Vector3.up*4, Color.magenta);
                for (int i = 1; i < pathToDraw.Length; ++i)
                {
                    Debug.DrawLine(pathToDraw[i], lastPos, PathColor);
                    lastPos = pathToDraw[i];

                    //Debug.DrawLine(pathToDraw[i], pathToDraw[i] + Vector3.up * 4, Color.magenta);
                }

                if (pathToDrawSmoothed != null)
                {
                    Vector3 lastPosSmoothed = pathToDrawSmoothed[0];

                    for (int i = 1; i < pathToDrawSmoothed.Length; ++i)
                    {
                        Debug.DrawLine(pathToDrawSmoothed[i], lastPosSmoothed, PathSmoothedColor);
                        lastPosSmoothed = pathToDrawSmoothed[i];
                    }
                }



            }

            // Draw Start and Goal
            Gizmos.color = GoalColor;
            Gizmos.DrawWireSphere(new Vector3(GoalPos.x * CellSize.x, 0, GoalPos.y * CellSize.y), CellSize.x / 2);
            Gizmos.color = StartColor;
            Gizmos.DrawWireSphere(new Vector3(StartPos.x * CellSize.x, 0, StartPos.y * CellSize.y), CellSize.x / 2);

            //// Draw Obstacles
            //if (pFObstacles != null && pFObstacles.Any())
            //{
            //    Gizmos.color = ObstacleColor;

            //    foreach (var ob in pFObstacles)
            //        ob.DebugDraw();
            //}


        }

        public int LOSroughCheckThreshold = 17;
        public bool IsLineOfSight(in Vector2Int start, in Vector2Int end)
        {
            Vector2Int l1 = start;
            Vector2Int l2 = end;

            int cellStartRough = pFPoints[start.x, start.y].Roughness;

            Vector2Int diff = l2 - l1;

            int f = 0;
            Vector2Int dir = Vector2Int.zero; // Direction of movement. Value can be either 1 or -1.

            Vector2Int offset = Vector2Int.zero;

            if (diff.y < 0)
            {
                diff.y = -diff.y;
                dir.y = -1;
                offset.y = 0; // Cell is to the North
            }
            else
            {
                dir.y = 1;
                offset.y = 1; // Cell is to the South
            }

            if (diff.x < 0)
            {
                diff.x = -diff.x;
                dir.x = -1;
                offset.x = 0; // Cell is to the West
            }
            else
            {
                dir.x = 1;
                offset.x = 1; // Cell is to the East
            }

            if (diff.x >= diff.y)
            { // Move along the x axis and increment/decrement y when f >= diff.x.
                while (l1.x != l2.x)
                {
                    f += diff.y;
                    if (f >= diff.x)
                    {  // We are changing rows, we might need to check two cells this iteration.
                        if (pFPoints[l1.x + offset.x, l1.y + offset.y].IsBlocked ||
                            Math.Abs(cellStartRough - pFPoints[l1.x + offset.x, l1.y + offset.y].Roughness) > LOSroughCheckThreshold)
                            return false;

                        l1.y += dir.y;
                        f -= diff.x;
                    }

                    if (f != 0 && (pFPoints[l1.x + offset.x, l1.y + offset.y].IsBlocked ||
                            Math.Abs(cellStartRough - pFPoints[l1.x + offset.x, l1.y + offset.y].Roughness) > LOSroughCheckThreshold))
                        return false;

                    // If we are moving along a horizontal line, either the north or the south cell should be unblocked.
                    if (diff.y == 0 && (pFPoints[l1.x + offset.x, l1.y].IsBlocked ||
                            Math.Abs(cellStartRough - pFPoints[l1.x + offset.x, l1.y].Roughness) > LOSroughCheckThreshold)
                       && (pFPoints[l1.x + offset.x, l1.y + 1].IsBlocked ||
                            Math.Abs(cellStartRough - pFPoints[l1.x + offset.x, l1.y + 1].Roughness) > LOSroughCheckThreshold)
                        )
                        return false;

                    l1.x += dir.x;
                }
            }
            else
            {  //if (diff.x < diff.y). Move along the y axis and increment/decrement x when f >= diff.y.
                while (l1.y != l2.y)
                {
                    f += diff.x;
                    if (f >= diff.y)
                    {
                        if (pFPoints[l1.x + offset.x, l1.y + offset.y].IsBlocked ||
                            Math.Abs(cellStartRough - pFPoints[l1.x + offset.x, l1.y + offset.y].Roughness) > LOSroughCheckThreshold)
                            return false;

                        l1.x += dir.x;
                        f -= diff.y;
                    }

                    if (f != 0 && (pFPoints[l1.x + offset.x, l1.y + offset.y].IsBlocked ||
                            Math.Abs(cellStartRough - pFPoints[l1.x + offset.x, l1.y + offset.y].Roughness) > LOSroughCheckThreshold))
                        return false;

                    if (diff.x == 0 &&
                        (pFPoints[l1.x, l1.y + offset.y].IsBlocked || Math.Abs(cellStartRough - pFPoints[l1.x, l1.y + offset.y].Roughness) > LOSroughCheckThreshold) &&
                        (pFPoints[l1.x + 1, l1.y + offset.y].IsBlocked || Math.Abs(cellStartRough - pFPoints[l1.x + 1, l1.y + offset.y].Roughness) > LOSroughCheckThreshold))
                        return false;

                    l1.y += dir.y;
                }
            }

            return true;
        }

        public bool IsLineOfSightJPS(in Vector2Int start, in Vector2Int end)
        {
            Vector2Int l1 = start;
            Vector2Int l2 = end;

            Vector2Int diff = l2 - l1;

            int f = 0;
            Vector2Int dir = Vector2Int.zero; // Direction of movement. Value can be either 1 or -1.

            Vector2Int offset = Vector2Int.zero;

            if (diff.y < 0)
            {
                diff.y = -diff.y;
                dir.y = -1;
                offset.y = 0; // Cell is to the North
            }
            else
            {
                dir.y = 1;
                offset.y = 1; // Cell is to the South
            }

            if (diff.x < 0)
            {
                diff.x = -diff.x;
                dir.x = -1;
                offset.x = 0; // Cell is to the West
            }
            else
            {
                dir.x = 1;
                offset.x = 1; // Cell is to the East
            }

            if (diff.x >= diff.y)
            { // Move along the x axis and increment/decrement y when f >= diff.x.
                while (l1.x != l2.x)
                {
                    f += diff.y;
                    if (f >= diff.x)
                    {  // We are changing rows, we might need to check two cells this iteration.
                        if (pFPoints[l1.x + offset.x, l1.y + offset.y].IsBlocked)
                            return false;

                        l1.y += dir.y;
                        f -= diff.x;
                    }

                    if (f != 0 && pFPoints[l1.x + offset.x, l1.y + offset.y].IsBlocked)
                        return false;

                    // If we are moving along a horizontal line, either the north or the south cell should be unblocked.
                    if (diff.y == 0 && pFPoints[l1.x + offset.x, l1.y].IsBlocked
                       && pFPoints[l1.x + offset.x, l1.y + 1].IsBlocked )
                        return false;

                    l1.x += dir.x;
                }
            }
            else
            {  //if (diff.x < diff.y). Move along the y axis and increment/decrement x when f >= diff.y.
                while (l1.y != l2.y)
                {
                    f += diff.x;
                    if (f >= diff.y)
                    {
                        if (pFPoints[l1.x + offset.x, l1.y + offset.y].IsBlocked)
                            return false;

                        l1.x += dir.x;
                        f -= diff.y;
                    }

                    if (f != 0 && pFPoints[l1.x + offset.x, l1.y + offset.y].IsBlocked)
                        return false;

                    if (diff.x == 0 &&
                        pFPoints[l1.x, l1.y + offset.y].IsBlocked &&
                        pFPoints[l1.x + 1, l1.y + offset.y].IsBlocked)
                        return false;

                    l1.y += dir.y;
                }
            }

            return true;
        }



        Vector3[] vertices;
        Vector2[] uvs;
        Color[] colors;
        int[] indices;
        public float ColorDiv = 7f;
        public Material TileMaterial;
        List<Mesh> meshes = new List<Mesh>(1);
        void GenMesh()
        {
            vertices = new Vector3[43400];
            uvs = new Vector2[43400];
            colors = new Color[43400];
            indices = new int[65000];

            int index = 0, indexB = 0;
            int quadFace = 0;
            for (int x = 0; x < XSize; ++x)
                for (int y = 0; y < YSize; ++y)
                {
                    if (indexB >= (65000 - 6))
                    {// 切换新模型
                        
                        var mesh = new Mesh();
                        mesh.vertices = vertices.ToArray();
                        mesh.uv = uvs.ToArray();
                        mesh.colors = colors.ToArray();
                        mesh.triangles = indices.Take(indexB).ToArray();
                        meshes.Add(mesh);

                        vertices = new Vector3[43400];
                        uvs = new Vector2[43400];
                        colors = new Color[43400];
                        indices = new int[65000];
                        index = indexB = quadFace = 0;
                    }

                    var point = pFPoints[x, y];
                    // TL, TR, BR, BL
                    vertices[index + 0] = new Vector3(x * CellSize.x - CellSize.x * 0.5f, 0, y * CellSize.y - CellSize.y * 0.5f);
                    vertices[index + 1] = new Vector3(x * CellSize.x + CellSize.x * 0.5f, 0, y * CellSize.y - CellSize.y * 0.5f);
                    vertices[index + 2] = new Vector3(x * CellSize.x + CellSize.x * 0.5f, 0, y * CellSize.y + CellSize.y * 0.5f);
                    vertices[index + 3] = new Vector3(x * CellSize.x - CellSize.x * 0.5f, 0, y * CellSize.y + CellSize.y * 0.5f);

                    uvs[index + 0] = new Vector2(x, y);
                    uvs[index + 1] = new Vector2(x + 1, y);
                    uvs[index + 2] = new Vector2(x + 1, y + 1);
                    uvs[index + 3] = new Vector2(x, y + 1);

                    colors[index + 0] = new Color(point.Roughness / ColorDiv, 1.0f - point.Roughness / ColorDiv, 1.0f - point.Roughness / ColorDiv);
                    colors[index + 1] = new Color(point.Roughness / ColorDiv, 1.0f - point.Roughness / ColorDiv, 1.0f - point.Roughness / ColorDiv);
                    colors[index + 2] = new Color(point.Roughness / ColorDiv, 1.0f - point.Roughness / ColorDiv, 1.0f - point.Roughness / ColorDiv);
                    colors[index + 3] = new Color(point.Roughness / ColorDiv, 1.0f - point.Roughness / ColorDiv, 1.0f - point.Roughness / ColorDiv);

                    indices[indexB + 0] = index + 0;
                    indices[indexB + 1] = index + 3;
                    indices[indexB + 2] = index + 1;

                    indices[indexB + 3] = index + 1;
                    indices[indexB + 4] = index + 3;
                    indices[indexB + 5] = index + 2;

                    quadFace += 1;
                    index = quadFace * 4;
                    indexB = quadFace * 6;
                }
            //Debug.Log(" " + quadFace + " " + index / 4 + " " + indexB / 6);

            var meshData = new Mesh();
            meshData.vertices = vertices.ToArray();
            meshData.uv = uvs.ToArray();
            meshData.colors = colors.ToArray();
            meshData.triangles = indices.Take(indexB).ToArray();
            meshes.Add(meshData);

            foreach (var mesh in meshes)
            {
                var meshGO = new GameObject();
                meshGO.transform.SetParent(this.transform);
                meshGO.transform.position = Vector3.zero;
                meshGO.transform.rotation = Quaternion.identity;
                meshGO.transform.localScale = Vector3.one;
                meshGO.AddComponent<MeshRenderer>().sharedMaterial = TileMaterial;
                meshGO.AddComponent<MeshFilter>().sharedMesh = mesh;

            }
            
            //Debug.Log(" " + quadFace + " " + index / 4 + " " + meshData.triangles.Length / 6);

        }


    }


    public class PFPoint
    {
        /// <summary>
        /// 我在PFGrid中的坐标
        /// </summary>
        public Vector2Int Pos;

        /// <summary>
        /// 邻居顶点
        /// </summary>
        public Vector2Int[] Neighbors;

        /// <summary>
        /// 通过性，0为障碍物
        /// </summary>
        public byte Passibility = 1;

        /// <summary>
        /// 通过这个地块产生的消耗
        /// </summary>
        public int Roughness = 1;

        public bool IsBlocked => Passibility == 0;


    }
}